import os
import pandas as pd
from collections import defaultdict

def parse_data(data):
    if len(data) < 30:  # 确保数据段至少有15个字节（30个字符）
        return None

    sof = data[0:2]
    
    # 提取第2-3个字节并调换顺序
    version_data = data[4:6] + data[2:4]
    # 将其解释为16进制数并转换为整数（大端模式）
    version_int = int(version_data, 16)
    # 提取高4位作为 version
    version = (version_int >> 12) & 0xF
    # 提取低12位作为 data_len
    data_len = version_int & 0x0FFF
    
    seq_number = int(data[8:10] + data[6:8], 16)
    robot_type = data[10:12]
    robot_id = data[12:14]
    rec_module_type = data[14:16]
    rec_module_id = int(data[16:18], 16) >> 3
    rec_module_subtype = int(data[16:18], 16) & 0x07
    send_module_type = data[18:20]
    send_module_id = int(data[20:22], 16) >> 3
    send_module_subtype = int(data[20:22], 16) & 0x07
    cmd_id = "0x" + data[24:26] + data[22:24]
    crc8 = data[26:28]
    user_data = data[28:28 + data_len * 2]
    crc16 = data[28 + data_len * 2:28 + data_len * 2 + 4]

    return {
        "SOF": sof,
        "Version": version,
        "Data Length": data_len,
        "Seq Number": seq_number,
        "Robot Type": robot_type,
        "Robot ID": robot_id,
        "Rec Module Type": rec_module_type,
        "Rec Module ID": rec_module_id,
        "Rec Module SubType": rec_module_subtype,
        "Send Module Type": send_module_type,
        "Send Module ID": send_module_id,
        "Send Module SubType": send_module_subtype,
        "Cmd ID": cmd_id,
        "CRC8": crc8,
        "User Data": user_data,
        "CRC16": crc16
    }

def process_file(input_file, output_file_txt, output_file_excel):
    can_data_dict = defaultdict(list)
    with open(input_file, 'r') as infile:
        lines = infile.readlines()
        for line in lines:
            line = line.strip()
            if not line:
                continue

            first_element_end = line.find(']') + 1
            if first_element_end == 0:
                continue

            first_element = line[:first_element_end]
            remaining_elements = line[first_element_end:].strip().split()

            if len(remaining_elements) != 3:
                continue

            time = first_element
            can_id, can_length, can_data = remaining_elements

            can_id = can_id.zfill(3)
            can_length = can_length.zfill(2)

            can_data_dict[can_id].append((time, can_id, can_length, can_data))

    processed_data = []
    with open(output_file_txt, 'w') as outfile:
        for can_id, entries in can_data_dict.items():
            i = 0
            while i < len(entries):
                time, can_id, can_length, can_data = entries[i]

                if can_data.startswith('5A'):
                    combined_data = can_data
                    j = i + 1
                    while j < len(entries):
                        next_time, next_can_id, next_can_length, next_can_data = entries[j]
                        if next_can_data.startswith('5A'):
                            break
                        combined_data += next_can_data
                        j += 1

                    outfile.write(f"{time} {can_id} {len(combined_data)//2:02} {combined_data}\n")
                    parsed_data = parse_data(combined_data)
                    if parsed_data:
                        processed_data.append([time, can_id, f"{len(combined_data)//2:02}", combined_data] + list(parsed_data.values()))
                    else:
                        processed_data.append([time, can_id, f"{len(combined_data)//2:02}", combined_data])
                    i = j
                else:
                    outfile.write(f"{time} {can_id} {can_length} {can_data}\n")
                    parsed_data = parse_data(can_data)
                    if parsed_data:
                        processed_data.append([time, can_id, can_length, can_data] + list(parsed_data.values()))
                    else:
                        processed_data.append([time, can_id, can_length, can_data])
                    i += 1

    columns = ["Time", "CAN ID", "Length", "Data"]
    if processed_data and len(processed_data[0]) > 4:
        columns += ["SOF", "Version", "Data Length", "Seq Number", "Robot Type", "Robot ID", "Rec Module Type", "Rec Module ID", "Rec Module SubType", "Send Module Type", "Send Module ID", "Send Module SubType", "Cmd ID", "CRC8", "User Data", "CRC16"]
    df = pd.DataFrame(processed_data, columns=columns)
    df.to_excel(output_file_excel, index=False)

if __name__ == "__main__":
    input_file = "/Users/flame.yu/flame_ws/rachel-debug-s3/tools/CAN000.txt"
    base, ext = os.path.splitext(input_file)
    output_file_txt = f"{base}-p{ext}"
    output_file_excel = f"{base}-p.xlsx"

    process_file(input_file, output_file_txt, output_file_excel)